#include "prospeckz.h"

#include <string.h>

#include "xlowpan.h"
#include "StopAndWait.h"

#include "dynmem.h"

#include "discovery.h"

struct xlowpan_addr64 serverAddr;
unsigned char serverHops;
unsigned char responsesCount;
struct xlowpan_addr64 discoResponses[DISC_BUFFER_SIZE];
char discoData[DISC_BUFFER_SIZE][14];
char discoMyID[14];

unsigned char disc_mode = DISC_MODE_IDLE;

unsigned char basestation = 0;

void (*discovery_accepted)(struct xlowpan_addr64* addr) = 0;

void discovery_set_data(char* data)
{
	int i = 0;
	while(*data != 0 && i < 14)
	{
		discoMyID[i++] = *(data++);
	}
}

void discovery_set_cdata(const char* data)
{
	int i = 0;
	while(*data != 0 && i < 14)
	{
		discoMyID[i++] = *(data++);
	}
}

char* discovery_get_data(unsigned char i)
{
	return discoData[i];
}

void discovery_init(void)
{
	struct xlowpan_addr64 address;
	E2PROM_E2Read(0, discoMyID, 14);
	E2PROM_E2Read(14, (unsigned char*)(&address),8);
	Reliable_Init(&address, 15);
}

void discovery_save(void)
{
	unsigned char buffer[22];
	m_memcpy(buffer, discoMyID, 14);
	
	Reliable_GetAddr((struct xlowpan_addr64*)buffer+14);
	
	E2PROM_bE2Write(0,discoMyID, 64, 25);
}

void discovery_find_basestation(void)
{	
	serverHops = 0;
	disc_mode = DISC_MODE_ASSOC_WAITING;

	discovery_find_basestation_beacon();
}

void discovery_find_basestation_beacon(void)
{
	unsigned char packet[3];

	packet[0] = DSP_DISCOVERY;
	packet[1] = BROADCAST_ASSOC;
	packet[2] = 0;
		
	xlowpan_send(&XLOWPAN_ADDR_BCAST, packet, 3);
}

struct xlowpan_addr64* discovery_find_basestation_end(void)
{
	disc_mode = DISC_MODE_IDLE;
	
	if(serverHops)
	{
		unsigned char buffer[3];
	
		buffer[0] = DSP_DISCOVERY;
		buffer[1] = BROADCAST_ASSOC_ACCEPT;
		buffer[2] = 0;
		
		//send an accept to the selected base station
		//retry on a reset: we don't care if this happens since we aren't 
		//keeping discovery state - we just want to make sure the bs knows 
		//we're here
		if(Reliable_TransmitPacket(&serverAddr, buffer, 3,40000,5) == FAIL_RESET) 
			Reliable_TransmitPacket(&serverAddr, buffer, 3,40000,5);
	
		return &serverAddr;
	}
	else
	{
		return NULL;
	}
}

void discovery_start(void)
{
	responsesCount = 0;
	disc_mode = DISC_MODE_DISC_WAITING;

	discovery_beacon();
}

void discovery_beacon(void)
{
	unsigned char packet[11];
	packet[0] = DSP_DISCOVERY;
	packet[1] = BROADCAST_DISC;
	packet[2] = serverHops > 0 ? TRANSMIT_ADDR : 0;
	xlowpan_addrcpy((struct xlowpan_addr64*)(packet+3), discovery_get_basestation());

	xlowpan_send(&XLOWPAN_ADDR_BCAST, packet, 11);
}

int discovery_end(struct xlowpan_addr64** addresses)
{
	disc_mode = DISC_MODE_IDLE;
	if(addresses != NULL)
	{
		*addresses = discoResponses;
	}
	return responsesCount;
}

static void discovery_respond(struct xlowpan_addr64* from, unsigned char type, struct xlowpan_addr64* station)
{		
	if(type == BROADCAST_ASSOC && basestation)
	{
		unsigned char packet[25];
		packet[0] = DSP_DISCOVERY;
		packet[1] = BROADCAST_ASSOC_RESPONSE;
		packet[2] = TRANSMIT_ADDR | TRANSMIT_DATA;
		xlowpan_addrcpy((struct xlowpan_addr64*)(packet+3),xlowpan_getaddr());
		m_memcpy(&(packet[11]),discoMyID, 14); 
		
		//retry on connection reset
		if(Reliable_TransmitPacket(from, packet, 25,40000,5) == FAIL_RESET)
		{
			Reliable_TransmitPacket(from, packet, 25,40000,5);
		}
	}
	//not a basestation, and either we are both on ad hoc mode, or the base stations match.
	else if((type == BROADCAST_DISC) && (!basestation) && ( (!serverHops && (station == NULL)) || !xlowpan_addrcmp(station, &serverAddr)) )
	{
		unsigned char packet[17];
		
		packet[0] = DSP_DISCOVERY;
		packet[1] = BROADCAST_DISC_RESPONSE;
		packet[2] = TRANSMIT_DATA;
		m_memcpy(&(packet[3]), discoMyID,14);
		
		//retry on connection reset
		if(Reliable_TransmitPacket(from, packet, 17,40000,5) == FAIL_RESET)
		{
			Reliable_TransmitPacket(from, packet, 17,40000,5);
		}
	}
}

static void discovery_responsereceived(struct xlowpan_addr64* from, unsigned char type, unsigned char* data, unsigned char hops, unsigned char length)
{
	//response from a device on our subnet
	if(type == BROADCAST_DISC_RESPONSE && disc_mode == DISC_MODE_DISC_WAITING)
	{	
		int i;
		if(responsesCount >= DISC_BUFFER_SIZE) return;
		//check to make sure we haven't already recorded this device's address
		for(i=0;i<responsesCount;i++)
		{
			if(!xlowpan_addrcmp(from, &discoResponses[i]))return;
		}
		i = 3;
		if(data[2] & TRANSMIT_ADDR)
		{
			i += 8;
		}
		if(data[2] & TRANSMIT_DATA)
		{

			m_memcpy(discoData[responsesCount], discoMyID, 14);
			i += 14;
		}
		
		discoResponses[responsesCount++] = *from;
		
	}
	//response from a base station
	if(type == BROADCAST_ASSOC_RESPONSE && disc_mode == DISC_MODE_ASSOC_WAITING)
	{
		if(serverHops != 0 && hops >= serverHops)
		{
			return;
		}
		
		//accept the closest server to respond
		serverAddr = *from;
		serverHops = hops;
	}
	
	if(basestation && type == BROADCAST_ASSOC_ACCEPT && discovery_accepted != NULL)
	{
		discovery_accepted(from);
	}
	
}

void discovery_decodepacket(struct xlowpan_addr64* from, unsigned char* data, unsigned char hops, unsigned char length)
{
	if(data[0] == DSP_DISCOVERY)
	{
		if(data[1] == BROADCAST_DISC || data[1] == BROADCAST_ASSOC)
		{
			discovery_respond(from, data[1], ((data[2] & TRANSMIT_ADDR) ? (struct xlowpan_addr64*)(data+3) : NULL));
		}
		else if(data[1] == BROADCAST_DISC_RESPONSE || data[1] == BROADCAST_ASSOC_RESPONSE)
		{
			discovery_responsereceived(from, data[1], data, hops, length-3);
		}
	}
}

struct xlowpan_addr64* discovery_get_basestation(void)
{
	if(serverHops)
		return &serverAddr;
	else return NULL;
}

void discovery_set_mode_basestation(unsigned char bs)
{
	basestation = bs;
}

void discovery_set_accept(void (*X)(struct xlowpan_addr64*))
{
	discovery_accepted = X;
}
